#include "armor_attitude_algorithm.h"
#include "debug.h"
 //#define FOCALLENGTH

/*
测试过程：
	1.打开 #define FOCALLENGTH 先测出实际距离，然后修改 “actualDist_”。（距离单位是mm）
	（选择的实际距离一般是测能达到的最大距离）
	2.通过调节滑动条，让焦距变得稳定，并修改焦距 “focalLenth_”。
测距方法：
	小孔成像原理
*/
ArrorAttitudeAlgorithm::ArrorAttitudeAlgorithm()
{
#ifdef FOCALLENGTH
	actualDist_=1600;
#endif
	ledHeight_=60;            //灯条高度

	inchPerPixWidth_ = (4.86 / 728.0);//(ccdWidth/imgWidth)
	inchPerPixHeight_ = (3.66 / 544.0);//(ccdHeight/imgHeight)

}
ArrorAttitudeAlgorithm::~ArrorAttitudeAlgorithm()
{
}

double ArrorAttitudeAlgorithm::angleSover(RotatedRect arrorRect, double & yaw, double & pitch, int enemy_color)
{
	if (enemy_color == 0)
	{
		focalLenth_ = 6.279412;      //实际焦距
	}
	else
	{
		focalLenth_ = 6.727941;      //实际焦距
	}
	

#ifdef FOCALLENGTH
	printf("实际焦距（实测参数代入）=%f\n",(arrorRect.size.height*inchPerPixHeight_)*actualDist_/ledHeight_);
#endif
	
	double diffX, diffY, distance;
	diffX = arrorRect.center.x - GWIDTH / 2.0;
	diffY = arrorRect.center.y - GHEIGHT / 2.0;
	yaw = atan(diffX * (inchPerPixWidth_ / focalLenth_)) * 57.2958;       //yaw轴角度
	pitch = atan(diffY * (inchPerPixHeight_ / focalLenth_)) * 57.2958;    //pitch轴角度


	distance = ((ledHeight_ * focalLenth_) / (arrorRect.size.height * inchPerPixHeight_ )/1000); //测得的距离 单位m
	return distance;
}
